// compile with -lbluetooth 
#include "send_receive.h"

#include <stdio.h>
#include <unistd.h>
#include <sys/socket.h>
#include <stdlib.h>
#include <error.h>
#include <arpa/inet.h>

#include <vector>

#include <bluetooth/bluetooth.h>
#include <bluetooth/rfcomm.h>


class PacketBuffer
{
public:
	struct packet { uint32_t sz; char payload[]; };
};

class PacketBufferIn : public PacketBuffer
{
private:
	std::vector<char> _buffer;

public:
	void append(const void *ptr, size_t size)
	{
		std::vector<char> v((const char*)ptr, (const char *)ptr + size);
		_buffer.insert(_buffer.end(), v.begin(), v.end());
	}

	bool hasPacket()
	{
		if (_buffer.size() < sizeof(packet)) {
			return false;
		}
		else {
			packet *p = (packet*)&_buffer[0];
			size_t msg_sz = ntohs(p->sz);
			return sizeof(packet) + msg_sz < _buffer.size();
		}
	}

	size_t nextPacket(void *buffer, size_t max_size)
	{
		packet *p = (packet*)&_buffer[0];
		size_t msg_sz = htonl(p->sz);
		if (msg_sz > max_size) msg_sz = max_size;
		memcpy(buffer, p->payload, msg_sz);
		return msg_sz;
	}
};


bool send(const char* msg, size_t msg_sz, const char dest[18], int channel)
{
    struct sockaddr_rc addr = { 0 };
    int s, status;

    printf("Server address: %s\n", dest);

    printf("creating socket...\n");
    // allocate a socket
    s = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
    printf("about to connect...\n");

    // set the connection parameters (who to connect to)
    addr.rc_family = AF_BLUETOOTH;
    addr.rc_channel = (uint8_t) channel;
    str2ba( dest, &addr.rc_bdaddr );

    // connect to server
    status = connect(s, (struct sockaddr *)&addr, sizeof(addr));
    printf("connected %d\n", status);

    char buf[1024];
    int n,i;

    typedef PacketBuffer::packet packet;

    // send a message
    if ( status == 0 ) {
        size_t sizeof_p =  sizeof(packet) + msg_sz;
        packet *p = (packet*)malloc(sizeof_p);
        memcpy(p->payload, msg, msg_sz);

        p->sz = htonl(msg_sz);
        status = write(s, p, sizeof_p);
        printf("sent %d\n", status);

        sleep(1);

        memset(buf,0, 1024);
        n = recv(s,buf,1024, 0);
        printf("got(%d): %s\n",n, buf);
        if (n < 0) {
          error(0,0,"recvfrom");
          return -1;
        }

        PacketBufferIn pbufin;
        pbufin.append(buf, n);

        if (!pbufin.hasPacket()) {
          error(0,0,"pbufin: packet is corrupted or truncated");
          return -1;
        }

        if (n < sizeof(packet)) {
          error(0,0,"packet is too short");
          return -1;
        }

        p = (packet*)buf;
        msg_sz = ntohl(p->sz);
        printf("packet size=%d\n", msg_sz);
        if (n < sizeof(packet) + msg_sz) {
          error(0,0,"packet is corrupted or truncated");
          return -1;
        }
        printf("payload =");
        for (int i = 0; i < msg_sz; ++i) {
          printf(" %c", p->payload[i]);
        }
        printf("\n");
    }
    else {
        perror("uh oh");
        return -1;
    }

    printf("disconnecting\n");

    close(s);
    return true;
}
